#include "FbxHelper.h"

namespace GC
{
	/// <summary>
	/// A list of all the FBX SDK Nodes so it creates a unique ID for each node
	/// The FBX NODE.GetID() functionality wasn't good enough
	/// </summary>
	std::vector<const FbxNode*> FbxHelper::s_nodeMap;
	const double FbxHelper::PI = 3.141592653589793238462;
	const double FbxHelper::Euler2Rad = FbxHelper::PI / 180;

	const int FbxHelper::DQLength = 8;
	const int FbxHelper::Matrix4x4Length = 16;

	//An empty constructor, just to hide it because the class is not meant to be constructed
	FbxHelper::FbxHelper()
	{
	}

	/// <summary>
	/// Returns the unique ID of an FbxNode	
	/// Returns -1 if the node doesn't exist (should NEVER happen)			
	/// </summary>
	/// <param name="a_node">The node</param>
	/// <returns></returns>
	const int FbxHelper::GetNodeID(const FbxNode* a_node)
	{
		if (a_node == NULL) return -1;

		const int f_nodeMapCount = s_nodeMap.size();

		for (int i = 0; i < f_nodeMapCount; ++i)
		{
			if (s_nodeMap[i] == a_node) 
			{
				return i;
			}
		}
		//If it could not find a node
		return -1;
	}

	/// <summary>
	/// Determines whether a_node is the root node or not by checking its ID
	/// </summary>
	/// <param name="a_node">The node to check</param>
	/// <returns></returns>
	const bool FbxHelper::IsRootNode(const FbxNode* a_node)
	{
		return GetNodeID(a_node) == 0;
	}

	
	/// <summary>
	/// Returns a vector that's either reserved for DQLength or Matrix4x4Length
	/// </summary>
	/// <returns></returns>
	VectorConstDouble FbxHelper::GetDQorMatrix4x4()
	{
		VectorConstDouble f_vector;		
		if ( ConverterApp::GetSettings()->IsUsingDualQuaternions() )
		{
			f_vector.reserve(DQLength);
		}
		else
		{
			f_vector.reserve(Matrix4x4Length);
		}

		return f_vector;
	}

	/// <summary>
	/// Gets the length of the vector that GetDQorMatrix4x4 returns
	/// </summary>
	/// <returns></returns>
	const int FbxHelper::GetDQorMatrix4x4Length()
	{
		if (ConverterApp::GetSettings()->IsUsingDualQuaternions())
		{
			return DQLength;
		}
		else
		{
			return Matrix4x4Length;
		}
	}
	
	/// <summary>
	///	Creates a Quaternion from radian/euler rotation based on wikipedias formula
	/// </summary>
	/// <param name="a_rotation">The rotation</param>
	/// <param name="a_isEuler">If rotation is euler degrees, if false it's radian</param>
	/// <returns>FbxQuaternion</returns>
	FbxQuaternion FbxHelper::GetQuaternion(const FbxVector4 a_rotation, const bool a_isEuler)
	{		
		double f_x =  a_rotation[0] * 0.5;    
		double f_y = a_rotation[1] * 0.5;
		double f_z = a_rotation[2] * 0.5;

		double sx, cx, sy, cy, sz, cz;
		//If euler, convert to radians
		if (a_isEuler)
		{
			f_x *= Euler2Rad;
			f_y *= Euler2Rad;
			f_z *= Euler2Rad;
		}
		
		sx = sin( f_x ); cx = cos( f_x ); 
		sy = sin( f_y ); cy = cos( f_y );
		sz = sin( f_z ); cz = cos( f_z );     
    
		double qx = ( sx * cy * cz ) - ( cx * sy * sz );
		double qy = ( cx * sy * cz ) + ( sx * cy * sz );
		double qz = ( cx * cy * sz ) - ( sx * sy * cz ); 
		double qw = ( cx * cy * cz ) + ( sx * sy * sz ); 

		return FbxQuaternion( qx, qy, qz, qw );
		
	}

	/// <summary>
	/// Sets DQ or Mat4x4 from a mat4x4
	/// </summary>
	/// <param name="a_src">The mat4x4 to copy</param>
	/// <param name="a_dest">The array/vector to copy to</param>
	void FbxHelper::SetDQorMat4x4FromMat(const FbxAMatrix a_src, VectorConstDouble& a_dest)
	{
		if (ConverterApp::GetSettings()->IsUsingDualQuaternions() )
		{
			SetDualQuaternion( a_src, a_dest );
		}
		else
		{
			//TODO: Implement matrix function
			ConverterApp::LogMessage(L"FAILED: TRIED TO SET MATRIX BUT NO FUNCTION EXISTS");
		}
	}

	/// <summary>
	/// Sets either DQ or Mat4x4 from a DQ
	/// </summary>
	/// <param name="a_src">The DQ to copy</param>
	/// <param name="a_dest">The destination array</param>
	void FbxHelper::SetDQorMat4x4FromDQ(const FbxDualQuaternion a_src, VectorConstDouble& a_dest)
	{
		if (ConverterApp::GetSettings()->IsUsingDualQuaternions() )
		{
			SetDualQuaternion( a_src, a_dest );
		}
		else
		{
			//TODO: Implement matrix function
			ConverterApp::LogMessage(L"FAILED: TRIED TO SET MATRIX BUT NO FUNCTION EXISTS");
		}
	}

	/// <summary>
	/// Sets a_dest as either DQ or Mat4 depending on ConverterSettings
	/// </summary>
	/// <param name="a_translation">The translation</param>
	/// <param name="a_rotation">The rotation</param>
	/// <param name="a_dest">The destination array</param>
	void FbxHelper::SetDQorMat4x4FromTR(const FbxVector4 a_translation, const FbxVector4 a_rotation, VectorConstDouble& a_dest)
	{
		if (ConverterApp::GetSettings()->IsUsingDualQuaternions())
		{
			FbxQuaternion f_rotQuat = GetQuaternion(a_rotation, true);

			FbxDualQuaternion f_dq = FbxDualQuaternion(f_rotQuat, a_translation);
			SetDQorMat4x4FromDQ(f_dq, a_dest);
		}
		//Matrixes
		else
		{
			//TODO: Implement matrix function
			ConverterApp::LogMessage(L"FAILED: TRIED TO SET MATRIX BUT NO FUNCTION EXISTS");
		}
	}

	/// <summary>
	/// Creates a DQ from a_src and then invokes SetDualQuaternion
	/// </summary>
	/// <param name="a_src">The matrix to get DQ from</param>
	/// <param name="a_dest">The array/vector to copy values into</param>
	void FbxHelper::SetDualQuaternion(const FbxAMatrix a_src, VectorConstDouble& a_dest)
	{
		FbxQuaternion f_quat = a_src.GetQ();
		FbxVector4 f_T = a_src.GetT();

		FbxDualQuaternion f_dualQuat = FbxDualQuaternion(f_quat, f_T);
		SetDualQuaternion( f_dualQuat, a_dest);
	}

	/// <summary>
	/// Copies values from FbxDualQuaternion to a_dest
	/// </summary>
	/// <param name="a_src">The DQ to copy</param>
	/// <param name="a_dest">The destination array/vector</param>
	void FbxHelper::SetDualQuaternion(const FbxDualQuaternion a_src, VectorConstDouble& a_dest)
	{
		const FbxQuaternion q1 = a_src.GetFirstQuaternion();

		a_dest.push_back(q1[0]);
		a_dest.push_back(q1[1]);
		a_dest.push_back(q1[2]);
		a_dest.push_back(q1[3]);

		const FbxQuaternion q2 = a_src.GetSecondQuaternion();

		a_dest.push_back(q2[0]);
		a_dest.push_back(q2[1]);
		a_dest.push_back(q2[2]);
		a_dest.push_back(q2[3]);		
	}

	/// <summary>
	/// Gets the geometry matrix for a node
	/// </summary>
	/// <param name="a_node">The a_node to get matrix from</param>	
	FbxAMatrix FbxHelper::GetGeometry(const FbxNode* a_node)
	{
		const FbxVector4 f_t = a_node->GetGeometricTranslation(FbxNode::eSourcePivot);
		const FbxVector4 f_r = a_node->GetGeometricRotation(FbxNode::eSourcePivot);
		const FbxVector4 f_s = a_node->GetGeometricScaling(FbxNode::eSourcePivot);
	
		return FbxAMatrix(f_t, f_r, f_s);
	}
	
	/// <summary>
	/// Gets the global position for a node using FbxPose
	/// It also uses Animation Evaluator
	/// </summary>
	/// <param name="a_node">The node to get global position for</param>
	/// <param name="a_pose">The possible pose, it can be null</param>
	/// <param name="a_parentGlobalPosition">The a_parent global position matrix, it's potentially recursively fetched</param>
	/// <returns></returns>
	FbxAMatrix FbxHelper::GetGlobalPosition(FbxNode* a_node, const FbxPose* a_pose, FbxAMatrix* a_parentGlobalPosition)
	{
		FbxAMatrix f_globalPosition;
		bool f_positionFound = false;
		
		if (a_pose)
		{
			int f_poseFindResult = a_pose->Find(a_node);

			if (f_poseFindResult > -1)
			{
				// The bind pose is always a global matrix.
				// If we have a rest pose, we need to check if it is
				// stored in global or local space.
				if (a_pose->IsBindPose() || !a_pose->IsLocalMatrix(f_poseFindResult))
				{
					f_globalPosition = GetPoseMatrix(a_pose, f_poseFindResult);
				}
				else
				{
					FbxAMatrix f_parentGlobalPosition;

					if (a_parentGlobalPosition)
					{
						f_parentGlobalPosition = *a_parentGlobalPosition;
					}
					else
					{
						//If node has a parent
						if (a_node->GetParent() )
						{
							//Call this function again
							f_parentGlobalPosition = GetGlobalPosition(a_node->GetParent(), a_pose);
						}
					}

					FbxAMatrix f_localPosition = GetPoseMatrix(a_pose, f_poseFindResult);
					f_globalPosition = f_parentGlobalPosition * f_localPosition;
				}

				f_positionFound = true;
			}			
		}
		//If no position is found then use the animation evaluator or something like that to get the globalPosition
		if (!f_positionFound)
		{
			FbxTime f_time;
			f_time.SetSecondDouble(0);
			try
			{						
				f_globalPosition = a_node->GetScene()->GetEvaluator()->GetNodeGlobalTransform( a_node );
			}
			catch (std::exception e)
			{
				#if defined (_DEBUG)
					Cout << _T("Failed: to get global position from scene evaluator");
				#endif
			}
		}

		return f_globalPosition;
	}

	/// <summary>
	/// Gets the pose matrix.
	/// </summary>
	/// <param name="a_pose">The pose to get matrix from</param>
	/// <param name="a_poseFindResult">The pose find result.</param>	
	FbxAMatrix FbxHelper::GetPoseMatrix(const FbxPose* a_pose, const int a_poseFindResult)
	{
		FbxAMatrix f_poseMatrix;
		FbxMatrix f_matrix = a_pose->GetMatrix(a_poseFindResult);

		memcpy((double*)f_poseMatrix, (double*)f_matrix, sizeof(f_matrix.mData));

		return f_poseMatrix;
	}

	/// <summary>
	/// Reserves the std::map size
	/// </summary>
	/// <param name="a_reserveSize">Size to reserve.</param>
	void FbxHelper::ReserveNodeMap(const int a_reserveSize)
	{
		s_nodeMap.reserve( a_reserveSize );
	}
	/// <summary>
	/// Adds the node to the unique nodeID map
	/// </summary>
	/// <param name="a_node">The a_node.</param>
	void FbxHelper::AddToMap(const FbxNode* a_node)
	{
		s_nodeMap.push_back( a_node );
	}

	/// <summary>
	/// Clears the unique ID node map.
	/// </summary>
	void FbxHelper::ClearNodeMap()
	{
		s_nodeMap.clear();
	}

	/***********************
		FbxPoseData Class
	*/

	/// <summary>
	/// Creates the PoseData class and figures out if there's a pose or not for the particular node
	/// </summary>
	/// <param name="a_node">The node to find a potential pose for</param>
	FbxHelper::FbxPoseData::FbxPoseData(const FbxNode* a_node)
	{
		if (a_node == NULL)
		{
			m_poseFindResult = -1;
			m_pose = nullptr;
			return;
		}

		FbxScene* f_scene = a_node->GetScene();

		const int f_poseCount = f_scene->GetPoseCount();

		for (int i = 0; i < f_poseCount; ++i)
		{
			const FbxPose* f_pose = f_scene->GetPose(i);
			const int f_poseFindResult = f_pose->Find( a_node );

			if (f_poseFindResult > -1)
			{
				m_pose = f_pose;
				m_poseFindResult = f_poseFindResult;

				return;
			}
		}

		m_pose = nullptr;
		m_poseFindResult = -1;
	}
}